using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using RobotSimulator.Basic;
using Unity.VisualScripting;
using static Unity.VisualScripting.Member;
public enum WheelMode
{
    Force,
    Speed,
    Angle
}
public class WheelActuator : BasicActuator
{

    public GameObject WheelAxis;
    public Rigidbody RealWheel;
    public float force;
    public float speed;
    public Pid speed_pid;
    public WheelMode wheelMode;
    protected override void Start()
    {
        if (rb == null)
        {
            rb = GetComponentInParent<BasicRobot>().rb;
        }
        if (WheelAxis.GetComponent<FixedJoint>() != null)
        {
            WheelAxis.GetComponent<FixedJoint>().connectedBody = rb;
        }
        base.Start();  
    }
    private FixAngle fixAngle=new();
    private float encoder_reg;
    private float speed_reg = 0;
    private float encoder_reg_last = 0;
    protected override void Loop()
    {
        encoder_reg = fixAngle.Fix(IMU.Euler(RealWheel.rotation).z);
        speed_reg = encoder_reg - encoder_reg_last;
        encoder_reg_last= encoder_reg;
        base.Loop();
        //Debug.Log(speed_reg);
        if (wheelMode == WheelMode.Force)
        {
            RealWheel.AddRelativeTorque(new Vector3(0, 0, force));
        }

        else if (wheelMode == WheelMode.Speed)
        {    
            force = speed_pid.Control(speed - (speed_reg>0?-1:1)*RealWheel.angularVelocity.magnitude);
            RealWheel.AddRelativeTorque(new Vector3(0, 0, force));      
        }
    }
}